#include "OI.h"
#include "Robotmap.h"
#include "Commands/GearBoxLowSpeed.h"
#include "Commands/GearBoxHighSpeed.h"
#include "Commands/DriveNormal.h"
#include "Commands/DriveReversed.h"
#include "Commands/PrecissionDriving.h"
#include "Commands/BallShootingControl.h"
#include "Commands/BallFeederControl.h"


#include "WPILib.h"


OI::OI():cypressIO(DriverStation::GetInstance()->GetEnhancedIO()) {
	
	isShooting = false;
	isPicking = false;
	
	
	
	// Process operator interface input here.
	//cypressIO = DriverStation::GetInstance()->GetEnhancedIO();

	leftStick = new Joystick(RobotMap::OP_CTRL_LEFT_STICK_PORT);
	rightStick = new Joystick(RobotMap::OP_CTRL_RIGHT_STICK_PORT );
	armStick = new Joystick(RobotMap::OP_CTRL_RAMP_ARM_STICK_PORT);
	
	lowSpeedBtn = new JoystickButton(leftStick, RobotMap::OP_CTRL_LOW_SPEED_BUTTON);
	highSpeedBtn = new JoystickButton(rightStick, RobotMap::OP_CTRL_HIGH_SPEED_BUTTON);
	
	forwardBtn = new JoystickButton(leftStick, RobotMap::OP_CTRL_DRIVE_FORWARD_BUTTON);
	reverseBtn = new JoystickButton(rightStick, RobotMap::OP_CTRL_DRIVE_REVERSE_BUTTON);
	
	precissionDrivingBtn = new DigitalIOButton(RobotMap::OP_CTRL_CY_PRECISION_DRIVING_ON_OFF);
	shootTheBallsBtn = new DigitalIOButton(RobotMap::OP_CTRL_CY_SHOOT_THE_BALLS);
	ballPickupOnOffBnt = new DigitalIOButton(RobotMap::OP_CTRL_CY_BALL_FEEDER_ON_OFF);
	
	assignCommands();
}

bool OI::isPickingBalls() {
	return isPicking;
}

bool OI::isShootingBalls() {
	return isShooting;
}

float OI::getLeftSpeed() {
	return leftStick->GetY();
}

float OI::getRightSpeed() {
	return rightStick->GetY();
}

float OI::getRampArmSpeed() {
	return armStick->GetY();
}

float OI::getCameraYAxis() {
	return leftStick->GetThrottle();
}

void OI::setPrecissionDrivingStatus(bool b) {
	cypressIO.SetDigitalOutput(RobotMap::OP_CTRL_CY_PRECISION_DRIVING_STAT, b);	
}

void OI::setBallFeederStatus(bool b) {
	cypressIO.SetDigitalOutput(RobotMap::OP_CTRL_CY_BALL_FEEDER_STAT, b);
	isPicking = b;
}

void OI::setBallOneStatus(bool b) {
	cypressIO.SetDigitalOutput(RobotMap::OP_CTRL_CY_BALL_1_STAT, b);
}


void OI::setBallTwoStatus(bool b) {
	cypressIO.SetDigitalOutput(RobotMap::OP_CTRL_CY_BALL_2_STAT, b);
}


void OI::setBallThreeStatus(bool b) {
	cypressIO.SetDigitalOutput(RobotMap::OP_CTRL_CY_BALL_3_STAT, b);
}

void OI::setShootingStatus(bool b) {
	cypressIO.SetDigitalOutput(RobotMap::OP_CTRL_CY_SHOOTING_STAT, b);
	isShooting = b;
}

float OI::getLowerFlyWheelSpeed() {
	return cypressIO.GetAnalogIn(RobotMap::OP_CTRL_CY_LOWER_FLYWHEEL_MOTOR_SPEED)/maximumVoltage;
}

float OI::getLowerFlyWheelSpeedRaw() {
	return cypressIO.GetAnalogIn(RobotMap::OP_CTRL_CY_LOWER_FLYWHEEL_MOTOR_SPEED);
}

float OI::getUpperFlyWheelSpeed() {
	return cypressIO.GetAnalogIn(RobotMap::OP_CTRL_CY_UPPER_FLYWHEEL_MOTOR_SPEED)/maximumVoltage;
}

float OI::getUpperFlyWheelSpeedRaw() {
	return cypressIO.GetAnalogIn(RobotMap::OP_CTRL_CY_UPPER_FLYWHEEL_MOTOR_SPEED);
}

void OI::assignCommands() {
	
	lowSpeedBtn->WhenPressed(new GearBoxLowSpeed());
	highSpeedBtn->WhenPressed(new GearBoxHighSpeed());
	forwardBtn->WhenPressed(new DriveNormal());
	reverseBtn->WhenPressed(new DriveReversed());
	
	precissionDrivingBtn->WhenPressed(new PrecissionDriving());
	        
	shootTheBallsBtn->WhenPressed(new BallShootingControl());
	ballPickupOnOffBnt->WhenPressed(new BallFeederControl());
	
}

